// FIXME: your file license if you have one

#define LOG_TAG "fht"
#define LOG_VERBOSE "fht"

#include "BearPi.h"
#include <hardware/hardware.h>
#include <hardware/bearpi.h>
#include <inttypes.h>
#include <unistd.h>


namespace vendor{
namespace hardware{
namespace bearpi{
namespace V1_0{
namespace implementation {


BearPi *BearPi::sInstance = nullptr;

BearPi::BearPi() :mClientCallback(nullptr), mDevice(nullptr){
    sInstance = this;
    mDevice = openHal();
    if(!mDevice){
        ALOGE("can't open Hal module");
    }
}

BearPi::~BearPi() {
    ALOGV("~BearPi()");
    if(mDevice == nullptr){
        ALOGE("No valid device");
        return;
    }

    mDevice = nullptr;
}

IBearPi* BearPi::getInstance() {
    if(!sInstance) {
        sInstance = new BearPi();
    }
    return sInstance;
}


bear_device_t* BearPi::openHal() {
    int err;
    const hw_module_t *hw_mdl= nullptr;
    ALOGD("Opening bear hal library...");
    err=hw_get_module(BEAR_HARDWARE_MODULE_ID, &hw_mdl);
    if(err != 0){
        ALOGE("Can't open bear hw module. error :%d",err);
        return nullptr;
    }

    if (hw_mdl == nullptr){
        ALOGE("No valid bear module");
        return nullptr;
    }

    bear_module_t const *module = reinterpret_cast<const bear_module_t*>(hw_mdl);
    if (module->common.methods->open == nullptr){
        ALOGE("No valid open method");
        return nullptr;
    }

    hw_device_t *device = nullptr;
    if (0 != (err=module->common.methods->open(hw_mdl, nullptr, &device))){
        ALOGE("Can't open bear methods, error:%d", err);
        return nullptr;
    }

    bear_device_t * bear_device = reinterpret_cast<bear_device_t*>(device);
    if (0 != (err=bear_device->set_notify(bear_device, BearPi::notify))){
        ALOGE("Can't register bear module callback, error: %d", err);
        return nullptr;
    }

    return bear_device;

}

void BearPi::notify(const bear_msg_t *msg) {
    BearPi* thisPtr = static_cast<BearPi*>(BearPi::getInstance());
    if (thisPtr == nullptr || thisPtr->mClientCallback == nullptr){
        ALOGE("Receiving callbacks before th client callback is registered.");
        return;
    }

    const uint64_t devId=reinterpret_cast<uint64_t>(thisPtr->mDevice);
    switch (msg->type) {
        case BEAR_ERROR:
            break;
        case BEAR_ACQUIRED:
            {
                int32_t vendorCode=0;
                BearPiAcquiredInfo result = VendorAcquiredFilter(msg->data.acquired.acquired_info, &vendorCode);
                ALOGD("Acquired(%d), vendorCode = %d", result,vendorCode);
                if(!thisPtr->mClientCallback->onResult(devId, result, vendorCode).isOk()) {
                    ALOGE("failed to invoke bear onResult callback");
                }
            }
            break;
        case BEAR_ACCELEANDGYRO:{
            hidl_vec<float> acceles = std::vector<float>(msg->acceles,msg->acceles+3);

            hidl_vec<float> gyros = std::vector<float>(msg->gyros, msg->gyros+3);

            ALOGD("bear accele and gyro data , accelex=%f, acceley=%f, accelez=%f, gyrox=%f, gyroy=%f, gyroz=%f",msg->acceles[0],
                    msg->acceles[1], msg->acceles[2], msg->gyros[0], msg->gyros[1], msg->gyros[2]);
            if(!thisPtr->mClientCallback->onAccelesAndGyroData(acceles, gyros).isOk()){
                ALOGE("failed to invoke bearpi onAccelesAndGyroData !!!");
            }
            }
            break;
        case BEAR_ANGLE_VALUE:
            ALOGD("BEAR ANGLE VALUE callback\n");
            if(!thisPtr->mClientCallback->onAngleValue(msg->pitch,msg->roll,msg->yaw).isOk()){
                ALOGE("failed to invoke bearpi onAngleValue !!!\n");
            }
            break;
        case BEAR_TEMP:
            ALOGD("BEAR TEMP :%f\n", msg->cpu_temp);
            if(!thisPtr->mClientCallback->onCpuTemperature(msg->cpu_temp).isOk()){
                ALOGE("failed to invoke \n");
            }
            break;
        default:
            break;
    }

}

BearPiAcquiredInfo BearPi::VendorAcquiredFilter(int32_t info, int32_t* vendorCode){
        *vendorCode = 0;
        switch(info) {
            case BEAR_ACQUIRED_GOOD:
                return BearPiAcquiredInfo::ACQUIRED_GOOD;
            case BEAR_ACQUIRED_PARTIAL:
                return BearPiAcquiredInfo::ACQUIRED_PARTIAL;
            default:
                if (info >= BEAR_ACQUIRED_VENDOR_BASE) {
                    // vendor specific code.
                    *vendorCode = info - BEAR_ACQUIRED_VENDOR_BASE;
                    return BearPiAcquiredInfo::ACQUIRED_VENDOR;
                }
        }
        ALOGE("Unknown acquiredmsg from bear vendor library: %d", info);
        return BearPiAcquiredInfo::ACQUIRED_GOOD;
}


// Methods from ::android::hardware::bearpi::V1_0::IBearPi follow.
Return<uint64_t> BearPi::setNotify(const sp<IBearPiClientCallback>& callback) {
    // TODO implement
    mClientCallback = callback;
    ALOGD("%s",__func__ );
    return reinterpret_cast<uint64_t>(mDevice);
}

Return<uint64_t> BearPi::test() {
    // TODO implement
    ALOGD("%s",__func__ );
    return mDevice->test(mDevice);
}

Return<uint64_t> BearPi::startGetAngle(){
    ALOGD("%s",__func__ );
    return mDevice->start_get_angle();
}

Return<uint64_t> BearPi::stopGetAngle(){
    ALOGD("%s",__func__ );
    return mDevice->stop_get_angle();
}
// Methods from ::android::hidl::base::V1_0::IBase follow.

//IBearPi* HIDL_FETCH_IBearPi(const char* /* name */) {
    //return new BearPi();
//}
//
}  // namespace implementation
}  // namespace V1_0
}  // namespace bearpi
}  // namespace hardware
}  // namespace vendor
